/*
 * cur_control.h
 *
 *  Created on: 2024年11月16日
 *      Author: zizhong
 */

#ifndef CONTROLLIB_CUR_CONTROL_H_
#define CONTROLLIB_CUR_CONTROL_H_

#include "foc_declare.h"
#include "svpwm.h"
#include "trans.h"
#include "PI.h"

extern float clarke1AlphaDlog;
extern float clarke1BetaDlog;
extern float UalphaDlog;
extern float UbetaDlog;


static inline void FCL_resetController(MOTOR_Vars_t *pMotor)
{
    pMotor->cmplx_Id.ref = 0.0;
    pMotor->cmplx_Id.carryOver = 0;
    pMotor->cmplx_Id.out = 0;
    pMotor->cmplx_Id.carryOver = 0;
    pMotor->cmplx_Id.err = 0;
    pMotor->cmplx_Id.xErr = 0;

    pMotor->cmplx_Iq.ref = 0.0;
    pMotor->cmplx_Iq.carryOver = 0;
    pMotor->cmplx_Iq.out = 0;
    pMotor->cmplx_Iq.carryOver = 0;
    pMotor->cmplx_Iq.err = 0;
    pMotor->cmplx_Iq.xErr = 0;

    pMotor->speedWePrev = 0;

    return;
}

static inline void FCL_runPICtrl(MOTOR_Vars_t *pMotor)
{
    register float  clarke1Alpha, clarke1Beta;
    register float  park1Sine, park1Cosine;
    SVM_ctl_t     svgen2;

    park1Sine   = __sinpuf32(pMotor->pangle);
    park1Cosine = __cospuf32(pMotor->pangle);

    //
    // CLARKE transformation
    //
    clarke1Alpha = (float)ADC_readPPBResult(ADCARESULT_BASE, ADC_PPB_NUMBER1) * pMotor->fclPara.adcPPBScale;

    clarke1Beta  = ((clarke1Alpha + (2.0F * ((float)(ADC_readPPBResult(ADCBRESULT_BASE, ADC_PPB_NUMBER1))
                    * pMotor->fclPara.adcPPBScale))) * 0.57735026918963); //ONEbySQRT3

    clarke1AlphaDlog = clarke1Alpha;
    clarke1BetaDlog = clarke1Beta;
    //
    // PARK Transformation
    //
    pMotor->cmplx_Iq.err = pMotor->cmplx_Iq.ref - ((clarke1Beta * park1Cosine) - (clarke1Alpha * park1Sine));

    pMotor->cmplx_Id.err = pMotor->cmplx_Id.ref - ((clarke1Alpha * park1Cosine) + (clarke1Beta * park1Sine));

    //
    // PI controllers for Id and Iq
    //
//    FCL_runPI(&pMotor->cmplx_Iq);     // Iq loop - PI controller - CPU
//    FCL_runPI(&pMotor->cmplx_Id);     // Id loop - PI controller - CPU

    //
    // Inverse Park Transformation
    //
    svgen2.Ualpha = ((pMotor->cmplx_Id.out * park1Cosine) - (pMotor->cmplx_Iq.out * park1Sine)) * pMotor->fclPara.carrierMid;

    svgen2.Ubeta  = ((pMotor->cmplx_Iq.out * park1Cosine) + (pMotor->cmplx_Id.out * park1Sine)) * pMotor->fclPara.cmidsqrt3;

    UalphaDlog = svgen2.Ualpha;
    UbetaDlog = svgen2.Ubeta;
    //
    // PWM pulse width time calculation
    //
    svgen2.Tb = (svgen2.Ubeta - svgen2.Ualpha) / 2;
    svgen2.Tc = svgen2.Tb - svgen2.Ubeta;

    svgen2.tmp2  = __fmax(__fmax(svgen2.Ualpha, svgen2.Tc), svgen2.Tb);
    svgen2.tmp2 += __fmin(__fmin(svgen2.Ualpha, svgen2.Tc), svgen2.Tb);
    svgen2.tmp1  = pMotor->fclPara.carrierMid - (svgen2.tmp2 / 2);

    //
    // PWM updates
    //
//    *(pMotor->pwmCompA) = (uint32_t)(svgen2.Tc + svgen2.tmp1);
//    *(pMotor->pwmCompB) = (uint32_t)(svgen2.Ualpha + svgen2.tmp1);
//    *(pMotor->pwmCompC) = (uint32_t)(svgen2.Tb + svgen2.tmp1);
    EPWM_setCounterCompareValue(myEPWM0_BASE, EPWM_COUNTER_COMPARE_A, (uint32_t)(svgen2.Tc + svgen2.tmp1));

    EPWM_setCounterCompareValue(myEPWM1_BASE, EPWM_COUNTER_COMPARE_A, (uint32_t)(svgen2.Ualpha + svgen2.tmp1));

    EPWM_setCounterCompareValue(myEPWM2_BASE, EPWM_COUNTER_COMPARE_A, (uint32_t)(svgen2.Tb + svgen2.tmp1));
}

//static inline void FCL_runAbsEncPICtrlWrap(MOTOR_Vars_t *pMotor)
//{
//    float Vbase = pMotor->fclPara.Vdcbus * 1.15f / 2.0f;
//    float invZbase = pMotor->fclPara.Ibase / Vbase;
//
//    // Update PI ID parameters
//    float wccXinvZb = invZbase * pMotor->fclPara.wccD;
//
//    pMotor->cmplx_Id.Kp = pMotor->fclPara.Ld * wccXinvZb;
//    pMotor->cmplx_Id.Ki = pMotor->fclPara.Rd * wccXinvZb * pMotor->fclPara.tSamp;
//
//    float Ki_rev = pMotor->cmplx_Id.Ki / 2;
//    pMotor->cmplx_Id.Kerr = Ki_rev + pMotor->cmplx_Id.Kp;
//    pMotor->cmplx_Id.KerrOld = Ki_rev - pMotor->cmplx_Id.Kp;
//
//    pMotor->cmplx_Id.carryOver = pMotor->cmplx_Id.err * pMotor->cmplx_Id.KerrOld;
//
//    // Update PI IQ parameters
//    wccXinvZb = invZbase * pMotor->fclPara.wccQ;
//
//    pMotor->cmplx_Iq.Kp = pMotor->fclPara.Lq * wccXinvZb;
//    pMotor->cmplx_Iq.Ki = pMotor->fclPara.Rq * wccXinvZb * pMotor->fclPara.tSamp;
//
//    Ki_rev = pMotor->cmplx_Iq.Ki / 2;
//
//    pMotor->cmplx_Iq.Kerr = Ki_rev + pMotor->cmplx_Iq.Kp;
//    pMotor->cmplx_Iq.KerrOld = Ki_rev - pMotor->cmplx_Iq.Kp;
//
//    pMotor->cmplx_Iq.carryOver =
//           (pMotor->cmplx_Iq.err * pMotor->cmplx_Iq.KerrOld) +
//           (pMotor->fclPara.BemfK * (pMotor->speedWe - pMotor->speedWePrev));
//
//    pMotor->speedWePrev = pMotor->speedWe;
//
//    // to pass on the id and iq current feedback back to user,
//    // update them in the wrap function
//    pMotor->cmplx_Id.fbk = pMotor->cmplx_Id.ref - pMotor->cmplx_Id.err;
//    pMotor->cmplx_Iq.fbk = pMotor->cmplx_Iq.ref - pMotor->cmplx_Iq.err;
//}


#endif /* CONTROLLIB_CUR_CONTROL_H_ */
